/*
 * fir.c
 *
 *  Created on: Jun 17, 2024
 *      Author: 17850
 */


/*

#include "fir.h"

// FIR 滤波器初始化函数

void arm_fir_init_f32(arm_fir_instance_f32 *S, uint16_t numTaps, float *pCoeffs, float *pState, uint32_t blockSize) {
    // 初始化滤波器结构体
    S->numTaps = numTaps;
    S->pCoeffs = pCoeffs;
    S->pState = pState;
    // 可选的初始化状态变量
    // arm_fill_f32(0.0f, S->pState, numTaps + blockSize - 1);
}
void arm_copy_f32(const float *src, float *dst, uint32_t length) {
    for (uint32_t i = 0; i < length; i++) {
        *dst++ = *src++;
    }
}

// FIR 滤波器处理函数
void arm_fir_f32(const arm_fir_instance_f32 *S, const float *pSrc, float *pDst, uint32_t blockSize) {
	float *pState = S->pState;  // 滤波器状态变量
    const float *pCoeffs = S->pCoeffs;  // 滤波器系数
    uint16_t numTaps = S->numTaps;  // 系数个数

    // 对每个输入样本进行处理
    for (uint32_t i = 0; i < blockSize; i++) {
        const float *pInput = pSrc + i;  // 输入样本指针
        float *pStateC = pState;  // 当前状态变量指针
        float sum = 0.0f;

        // 计算输出样本
        for (uint16_t j = 0; j < numTaps; j++) {
            sum += *pInput * *pCoeffs++;
            pInput -= 1;
        }

        // 更新状态变量
        *pStateC++ = *pInput;

        // 将计算结果存储到输出数组中
        *pDst++ = sum;
    }

    // 更新状态变量数组
    arm_copy_f32(pState + blockSize, pState, numTaps - 1);
}


*/

///////////////////

#include <string.h>
#include "fir.h"

// FIR 滤波器初始化函数
void arm_fir_init_f32(
    arm_fir_instance_f32 *S,
    uint16_t numTaps,
    const float *pCoeffs,
    float *pState,
    uint32_t blockSize)
{
    S->numTaps = numTaps;
    S->pCoeffs = (float *)pCoeffs;
    S->pState = pState;
    memset(pState, 0, (numTaps + blockSize - 1U) * sizeof(float));
}

// FIR 滤波器执行函数
void arm_fir_f32(
    const arm_fir_instance_f32 *S,
    const float *pSrc,
	float *pDst,
    uint32_t blockSize)
{
	float *pState = S->pState;        // State pointer
    const float *pCoeffs = S->pCoeffs; // Coefficient pointer
    float *pStateCurnt;               // Points to the current sample of the state
    float *px, *pb;                   // Temporary pointers for state and coefficient buffers
    uint32_t numTaps = S->numTaps;        // Number of filter coefficients in the filter
    uint32_t tapCnt, blkCnt;              // Loop counters

    // Initialize the state pointer
    pStateCurnt = S->pState + (numTaps - 1U);

    // Process each blockSize samples
    blkCnt = blockSize;
    while (blkCnt > 0U)
    {
        // Copy new input sample into the state buffer
        *pStateCurnt++ = *pSrc++;

        // Initialize pointers to state and coefficient buffers
        px = pState;
        pb = (float *)pCoeffs;

        // Initialize accumulator
        float acc = 0.0f;

        // Perform the multiply-accumulate operation
        tapCnt = numTaps;
        while (tapCnt > 0U)
        {
            acc += (*px++) * (*pb++);
            tapCnt--;
        }

        // Store the result in the destination buffer
        *pDst++ = acc;

        // Advance the state pointer to process the next sample
        pState++;

        // Decrement the blockSize loop counter
        blkCnt--;
    }

    // Copy the remaining state variables to the start of the state buffer
    // to prepare for the next block of samples
    memmove(S->pState, S->pState + blockSize, (numTaps - 1U) * sizeof(float));
}

